#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include "uart.h"
#include "motor.h"
#include "joystick.h"
#include "common.h"
#include "packet.h"
#include "radio.h"
#include "os.h"

#define debug_sensors 1
#define debug_motors 1
#define debug_strings 1

const unsigned char PPP[] = {};
const unsigned int PT = 0; 

const unsigned short send_address = HOVER_ADDRESS;
const unsigned short recv_address = BASE_ADDRESS;

volatile uint8_t recv_buffer[PAYLOAD_BYTES];
volatile uint8_t data_recv = 0;

int main(){

	DISABLE_INTERRUPTS();
	DISABLE_PRESCALER();
	DDRD = 0xff;

	Reset_Now(0);
	radio_init();
	radio_set_receive();
	radio_set_receive();

	uart_init();
	ENABLE_INTERRUPTS();

	if(debug_strings)
		printf("Hello from base station\n");

	for(;;){

		// blink green (active)
		PORTD ^= _BV(PORTD5);

		// receive debugging data from hover
		if(data_recv==1){
			data_recv = 0;
			int type = recv_buffer[0];
			int i;

			// string data
			if(type==0 && debug_strings ){
				printf("%u ", Now());
				for(i=1; i<PAYLOAD_BYTES && recv_buffer[i]; i++)
					printf("%c",(char)recv_buffer[i]);
				printf("\n");
			}

			// sensor data	
			else if(type==1 && debug_sensors ){
				printf("%u ", Now());
				for(i=1; i<PAYLOAD_BYTES; i++)
					printf("%i ",recv_buffer[i]);
				printf("\n");
			}

			// motor data
			else if(type==2 && debug_motors ){
				printf("%u ", Now());
				for(i=1; i<PAYLOAD_BYTES; i++)
					printf("%i ",recv_buffer[i]);
				printf("\n");
			}
			data_recv = 0;
		}
		_delay_ms(50);
	}

    return 0;
}

/**
 * receive radio packet interrupt
 **/
ISR(INT4_vect){
    int i;
    PORTD ^= _BV(PORTD7); // blink red (receive)

    for (i = 0; i < PAYLOAD_BYTES; i++)
        recv_buffer[i] = radio_get_byte();

    data_recv = 1;
    radio_set_receive();
    radio_set_receive();
}

